
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       gp_vector3.c
  * @author     baiyang
  * @date       2021-7-6
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "gp_math.h"
#include "gp_vector3.h"
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/** 
  * @brief       三维向量赋值
  * @param[in]   vo  
  * @param[in]   x  
  * @param[in]   y  
  * @param[in]   z  
  * @param[out]  
  * @retval      
  * @note        
  */
void vec3_set(Vector3f_t* vo, const float x, const float y, const float z)
{
    vo->x = x;
    vo->y = y;
    vo->z = z;
}

/** 
  * @brief       三维向量初始化为0
  * @param[in]   vo
  * @param[out]  
  * @retval      
  * @note        
  */
void vec3_zero(Vector3f_t* vo)
{
    vo->x = 0.0f;
    vo->y = 0.0f;
    vo->z = 0.0f;
}

/** 
  * @brief       计算三维向量模值
  * @param[in]   v  
  * @param[out]  
  * @retval      
  * @note        
  */
float vec3_length(const Vector3f_t* v)
{
    return sqrtf(v->x * v->x + v->y * v->y + v->z * v->z); 
}

float vec3_length_xy(const Vector3f_t* v)
{
    return sqrtf(v->x * v->x + v->y * v->y); 
}

// limit xy component vector to a given length. returns true if vector was limited
bool vec3_limit_length_xy(Vector3f_t* v, float max_length)
{
    const float length_xy = sqrtf(v->x * v->x + v->y * v->y);
    if ((length_xy > max_length) && math_flt_positive(length_xy)) {
        v->x *= (max_length / length_xy);
        v->y *= (max_length / length_xy);
        return true;
    }
    return false;
}

/** 
  * @brief       计算三维向量模值的平方
  * @param[in]   v  
  * @param[out]  
  * @retval      
  * @note        
  */
float vec3_length_squared(const Vector3f_t* v)
{
    return v->x * v->x + v->y * v->y + v->z * v->z; 
}

/** 
  * @brief       三维向量单位化
  * @param[in]   vo  
  * @param[in]   vi  
  * @param[out]  
  * @retval      
  * @note        
  */
void vec3_norm(Vector3f_t* vo, const Vector3f_t* vi)
{
    float len = vec3_length(vi);
    if(math_flt_zero(len))
    {
        vec3_zero(vo);
    }
    else
    {
        vo->x = vi->x / len;
        vo->y = vi->y / len;
        vo->z = vi->z / len;
    }
}

/** 
  * @brief       三维向量加运算(vo = v1 + v2)
  * @param[in]   vo  
  * @param[in]   v1  
  * @param[in]   v2  
  * @param[out]  
  * @retval      
  * @note        
  */
void vec3_add(Vector3f_t* vo, const Vector3f_t* v1, const Vector3f_t* v2)
{
    vo->x = v1->x + v2->x;
    vo->y = v1->y + v2->y;
    vo->z = v1->z + v2->z;
}

/** 
  * @brief       三维向量减运算（vo = v1 - v2）
  * @param[in]   vo  
  * @param[in]   v1  
  * @param[in]   v2  
  * @param[out]  
  * @retval      
  * @note        
  */
void vec3_sub(Vector3f_t* vo, const Vector3f_t* v1, const Vector3f_t* v2)
{
    vo->vec3[0] = v1->vec3[0] - v2->vec3[0];
    vo->vec3[1] = v1->vec3[1] - v2->vec3[1];
    vo->vec3[2] = v1->vec3[2] - v2->vec3[2];
}

/** 
  * @brief       三维向量减运算（vo = v1 - v2）
  * @param[in]   vo  
  * @param[in]   v1  
  * @param[in]   v2  
  * @param[out]  
  * @retval      
  * @note        
  */
Vector3f_t vec3_sub2(const Vector3f_t* v1, const Vector3f_t* v2)
{
    Vector3f_t vo;

    vo.vec3[0] = v1->vec3[0] - v2->vec3[0];
    vo.vec3[1] = v1->vec3[1] - v2->vec3[1];
    vo.vec3[2] = v1->vec3[2] - v2->vec3[2];

    return vo;
}

/** 
  * @brief       叉乘运算
  * @param[in]   vo  
  * @param[in]   vl  
  * @param[in]   vr  
  * @param[out]  
  * @retval      
  * @note        
  */
void vec3_cross(Vector3f_t* vo, const Vector3f_t* vl, const Vector3f_t* vr)
{
    Vector3f_t vl_tmp = *vl;
    Vector3f_t vr_tmp = *vr;

    vo->vec3[0] = vl_tmp.vec3[1]*vr_tmp.vec3[2] - vl_tmp.vec3[2]*vr_tmp.vec3[1];
    vo->vec3[1] = vl_tmp.vec3[2]*vr_tmp.vec3[0] - vl_tmp.vec3[0]*vr_tmp.vec3[2];
    vo->vec3[2] = vl_tmp.vec3[0]*vr_tmp.vec3[1] - vl_tmp.vec3[1]*vr_tmp.vec3[0];
}

/** 
  * @brief       点乘运算
  * @param[in]   v1  
  * @param[in]   v2  
  * @param[out]  
  * @retval      
  * @note        
  */
float vec3_dot(const Vector3f_t* v1, const Vector3f_t* v2)
{
    return v1->vec3[0] * v2->vec3[0] + v1->vec3[1] * v2->vec3[1] + v1->vec3[2] * v2->vec3[2];
}

/** 
  * @brief       运放（vo = vi * scalar）
  * @param[in]   vo  
  * @param[in]   vi  
  * @param[in]   scalar  
  * @param[out]  
  * @retval      
  * @note        
  */
void vec3_mult(Vector3f_t* vo, const Vector3f_t* vi, const float scalar)
{
    vo->vec3[0] = vi->vec3[0] * scalar;
    vo->vec3[1] = vi->vec3[1] * scalar;
    vo->vec3[2] = vi->vec3[2] * scalar;
}

/** 
  * @brief       运放（vo = vi * scalar）
  * @param[in]   vo  
  * @param[in]   vi  
  * @param[in]   scalar  
  * @param[out]  
  * @retval      
  * @note        
  */
Vector3f_t vec3_mult2(const Vector3f_t* vi, const float scalar)
{
    Vector3f_t vo;

    vo.vec3[0] = vi->vec3[0] * scalar;
    vo.vec3[1] = vi->vec3[1] * scalar;
    vo.vec3[2] = vi->vec3[2] * scalar;

    return vo;
}

/**
  * @brief       
  * @param[in]   vo  
  * @param[in]   vl  
  * @param[in]   vr  
  * @param[out]  
  * @retval      
  * @note        
  */
void vec3_hadamard_product(Vector3f_t* vo, const Vector3f_t* vl, const Vector3f_t* vr)
{
    vo->vec3[0] = vl->vec3[0] * vr->vec3[0];
    vo->vec3[1] = vl->vec3[1] * vr->vec3[1];
    vo->vec3[2] = vl->vec3[2] * vr->vec3[2];
}

// return bearing in centi-degrees between two positions
float vec3_get_bearing_cd(const Vector3f_t *origin, const Vector3f_t *destination)
{
    float bearing = atan2f(destination->y-origin->y, destination->x-origin->x) * DEGX100;
    if (bearing < 0) {
        bearing += 36000.0f;
    }
    return bearing;
}

/**
  * @brief       
  * @param[in]   v  
  * @param[out]  
  * @retval      
  * @note        
  */
float vec3_angle_xy(const Vector3f_t *v)
{
    return M_PI_2 + atan2f(-v->x, v->y);
}
/*------------------------------------test------------------------------------*/


